
// 1. 核心控制逻辑说明：

// 中心点接收：订阅由<center_detection>功能包发布的中心点坐标，并对其进行json解析
// 点云滤波：采用移动平均滤波函数对中心点累积取平均，用来计算距离
// 偏移计算：根据当前中心点坐标计算其与坐标系原点之间的距离
// 控制阶段：根据偏移量计算相应的速度指令
// 注意：因为该阶段的偏移量很小，所以生成的速度值一定要很小，才能保证能精准地对齐

// 2. 速度调整策略：

// X偏移 → 控制前进/后退速度
// Y偏移 → 控制左右平移速度
// 角度偏移（无需角度偏移）


#include "local_navigation/local_navigator.hpp"
#include <jsoncpp/json/json.h>
#include <rclcpp/rclcpp.hpp>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <deque>

namespace local_navigation {

    LocalNavigator::LocalNavigator(const rclcpp::NodeOptions & options)
        : Node("local_navigator", options),
        current_center_x_(0.0),
        current_center_y_(0.0),
        current_center_z_(0.0),
        target_detected_(false),
        x_buffer_(),
        y_buffer_(),
        z_buffer_()
    {
        // 声明并获取参数
        center_json_topic_ = this->declare_parameter("center_json_topic", "/barrel_center_json");
        cmd_vel_topic_ = this->declare_parameter("cmd_vel_topic", "/cmd_vel");

        max_linear_velocity_ = this->declare_parameter("max_linear_velocity", 0.1); 
        position_tolerance_ = this->declare_parameter("position_tolerance", 0.02); 
        FILTER_WINDOW_SIZE_ = this->declare_parameter("filter_window_size", 3);
        

        // 创建发布者和订阅者
        center_json_sub_ = this->create_subscription<std_msgs::msg::String>(center_json_topic_, 10,
            std::bind(&LocalNavigator::centerJsonCallback, this, std::placeholders::_1));

        cmd_vel_pub_ = this->create_publisher<std_msgs::msg::String>(cmd_vel_topic_, 10);
        
        // 创建控制循环定时器 
        control_timer_ = this->create_wall_timer(std::chrono::milliseconds(200),
            std::bind(&LocalNavigator::controlLoop, this));

        RCLCPP_INFO(this->get_logger(), "Fine Adjustment Controller initialized.");
    }


    // 解析、传递订阅的放射源中心点坐标
    void LocalNavigator::centerJsonCallback(const std_msgs::msg::String::SharedPtr msg)
    {
        
        if(center_coords.parse_json_string(msg->data))
        {
            current_center_x_ = center_coords.get_coord_x();
            current_center_y_ = center_coords.get_coord_y();
            current_center_z_ = center_coords.get_coord_z();

            // 将新值添加到缓冲区
            x_buffer_.push_back(current_center_x_);
            y_buffer_.push_back(current_center_y_);
            z_buffer_.push_back(current_center_z_);

            // 如果缓冲区大小超过窗口大小，则移除最旧的元素
            if (x_buffer_.size() > FILTER_WINDOW_SIZE_)
            {
                x_buffer_.pop_front();
                y_buffer_.pop_front();
                z_buffer_.pop_front();
            }

            target_detected_ = true;
        }
        else
        {
            target_detected_ = false;
        }

    }


    // 控制loop
    void LocalNavigator::controlLoop()
    {

        if (!target_detected_) 
        {
            // 没有检测到目标，停止
            double linear_x = 0.0;
            double linear_y = 0.0;
            double angular_z = 0.0;
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No target detected");
        } else 
        {
            // 只有当缓冲区中有数据才进行计算
            if(!x_buffer_.empty()){
                // 计算缓冲区中 X， Y， Z 坐标的平均值
                double avg_x = 0.0, avg_y = 0.0, avg_z = 0.0;
                for(double val : x_buffer_) {avg_x += val;}
                for(double val : y_buffer_) {avg_y += val;}
                for(double val : z_buffer_) {avg_z += val;}
                avg_x /= x_buffer_.size();
                avg_y /= y_buffer_.size();
                avg_z /= z_buffer_.size();

                calculateVelocityFromDistance(linear_x, linear_y, angular_z, avg_x, avg_y);
            }

        }

        vel_cmd.set_from_twist(linear_x, linear_y, angular_z);
        std::string json_string = vel_cmd.get_json_string();

        if(!json_string.empty() && json_string != "null") {
            std_msgs::msg::String msg;
            msg.data = json_string;
            cmd_vel_pub_->publish(msg);
        }

    }



    // 根据放射源中心点与原点距离输出x、y轴速度分量
    // 添加了avg_x、avg_y作为输入参数
    void LocalNavigator::calculateVelocityFromDistance(double& linear_x, double& linear_y, double& angular_z, double avg_x, double avg_y)
    {
        // 使用平均后的坐标计算距离
        double distance = std::sqrt(avg_x * avg_x + avg_y * avg_y);

        if(distance > position_tolerance_ && distance > 1e-6){
            linear_x = max_linear_velocity_ * (avg_x / distance);
            linear_y = max_linear_velocity_ * (avg_y / distance);
            angular_z = 0;

        } else {
            linear_x = 0.0; // 到达目标时停止
            linear_y = 0.0;
            angular_z = 0.0;
        }
    
    }

} 

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<local_navigation::LocalNavigator>());
    rclcpp::shutdown();
    return 0;
}